#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h> // 用于转换为欧拉角

int main(int argc, char** argv){
  ros::init(argc, argv, "Trans_TF_2d");

  ros::NodeHandle node;

  tf::TransformListener listener;
  tf::TransformBroadcaster broadcaster;
  tf::Transform transform_broadcaster;
  ros::Duration(10.0).sleep();
  int counter=0;

  ros::Rate rate(50);
  while (node.ok()){
    tf::StampedTransform transform_listener;
    
    try{
      listener.lookupTransform("map", "body",  
                               ros::Time(0), transform_listener);
    }
    catch (tf::TransformException ex){
      ROS_ERROR("ERROR from trans_tf_2d: %s",ex.what());
      ros::Duration(0.5).sleep();
      continue;
    }

    float robot_pose_x = transform_listener.getOrigin().x();
    float robot_pose_y = transform_listener.getOrigin().y();
    float robot_pose_z = transform_listener.getOrigin().z();

    float robot_oriation_x = transform_listener.getRotation().getX();
    float robot_oriation_y = transform_listener.getRotation().getY();
    float robot_oriation_z = transform_listener.getRotation().getZ();
    float robot_oriation_w = transform_listener.getRotation().getW();

    // 将四元数转换为欧拉角
    tf::Quaternion quat(robot_oriation_x, robot_oriation_y, robot_oriation_z, robot_oriation_w);
    double roll, pitch, yaw;
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

    // if( counter <= 0)
    // {
    //   ROS_INFO("Transform /map to /body: x=%.3f, y=%.3f, z=%.3f, roll=%.3f, pitch=%.3f, yaw=%.3f",
    //            robot_pose_x, robot_pose_y, robot_pose_z, roll, pitch, yaw);
    //   counter=5000;
    // }

    transform_broadcaster.setOrigin( tf::Vector3(robot_pose_x, robot_pose_y, 0.0) );
    transform_broadcaster.setRotation( tf::Quaternion(0, 0, robot_oriation_z, robot_oriation_w) );
    broadcaster.sendTransform(tf::StampedTransform(transform_broadcaster, ros::Time::now(), "map", "body_2d"));

    rate.sleep();
    counter--;
  }
  return 0;
};